#ifndef FLASHROS_UTILS_H
#define FLASHROS_UTILS_H

#include <INIReader.h>
#include <ros/ros.h>
#include <signal.h>
#include <unistd.h>

#include <ctime>
#include <fstream>
#include <iomanip>
#include <string>

namespace flashros {
namespace commons {

inline void RosShutdown() {
  ROS_INFO("shutting down!");
  ros::shutdown();
  exit(0);
}

inline void CheckFileExists(std::string& file_path) {
  if (access(file_path.c_str(), F_OK) == -1) {
    ROS_ERROR_STREAM("File " << file_path << " doesn't exists.");
    RosShutdown();
  }
}

inline std::string GetTimePointString(
    const char* format = "%Y-%m-%d %H:%M:%S") {
  std::stringstream ss;
  std::time_t t = std::time(NULL);
  std::tm tm = *std::localtime(&t);
  ss << std::put_time(&tm, format);
  return ss.str();
}

inline void CustomSigintHandler(int sig) { RosShutdown(); }

inline void RosInit(int argc, char** argv, std::string node_name) {
  ros::init(argc, argv, node_name);
  signal(SIGINT, CustomSigintHandler);
  ROS_INFO_STREAM("Init " << node_name << " Node");
  ROS_DEBUG("Debug Mode...");
}

inline std::string GetRosParam(ros::NodeHandle& nh,
                               const std::string param_name) {
  std::string param;
  nh.param<std::string>(param_name, param, "");
  ROS_DEBUG_STREAM("Read ROS Param " << param_name << ": " << param);
  return param;
}

inline INIReader* InitConfig(ros::NodeHandle& nh,
                             const std::string arg_name = "config_path") {
  std::string config_path = GetRosParam(nh, arg_name);
  CheckFileExists(config_path);
  ROS_INFO_STREAM("Find config file " << config_path);
  return new INIReader(config_path);
}

inline void ParseClassNames(std::vector<std::string>& class_names,
                            const std::string& label_file_path) {
  std::string line;
  std::ifstream label_file(label_file_path);
  if (!label_file.good()) {
    ROS_ERROR_STREAM("Object label file " << label_file_path
                                          << " doesn't exist");
  }
  while (!label_file.eof()) {
    std::getline(label_file, line, '\n');
    if (line.length() == 0) continue;
    class_names.push_back(line);
  }
}

}  // namespace commons
}  // namespace flashros

#endif